#include "plcdataprocess.h"
#include <log/log.h>

namespace HIM{

int16_t PlcDataProcess::GetPlcBitData(AddressIndex ind , uint16_t PlcReadData[100])
{
    if(!PLCCONNECTER.getConnectStatus())
        return 0;

    unsigned int i=ind>>4;
    unsigned int bit_loc=ind&0x0f;
    return getBitFromDByte(PlcReadData+i-read_starting_addr,bit_loc);
}

int16_t PlcDataProcess::GetPlcWordData(AddressIndex ind , uint16_t PlcReadData[100])
{
    if(!PLCCONNECTER.getConnectStatus())
        return 0;

    unsigned int i=ind>>4;
    return *(PlcReadData+i-read_starting_addr);
}

float PlcDataProcess::GetPlcFloatData(AddressIndex ind , uint16_t PlcReadData[100])
{
    if(!PLCCONNECTER.getConnectStatus())
        return 0;

    unsigned int i=ind>>4;
    int add = i-read_starting_addr;

    uint16_t src[2] ;
    src[0] = PlcReadData[add];
    src[1] = PlcReadData[add+1];
    return  PLCCONNECTER.getFloatFromDDByte(src);
}

void PlcDataProcess::addSetPlcBitDataTask(AddressIndex ind,bool bit)
{
    if(!PLCCONNECTER.getConnectStatus())
        return;

    PLCTask subTask;
    subTask.addressIndex=ind;
    subTask.dataType = BIT;
    subTask.bit = bit;
    plcTaskList.try_enqueue(subTask);
}

void PlcDataProcess::addSetPlcWordDataTask(AddressIndex ind,int16_t data)
{
    if(!PLCCONNECTER.getConnectStatus())
        return;
    PLCTask subTask;
    subTask.addressIndex=ind;
    subTask.dataType = INT_16;
    subTask.int16 = data;
    plcTaskList.try_enqueue(subTask);
}

void PlcDataProcess::addSetPlcFloatDataTask(AddressIndex ind,float data)
{
    if(!PLCCONNECTER.getConnectStatus())
        return;
    PLCTask subTask;
    subTask.addressIndex=ind;
    subTask.dataType = FLOAT_32;
    subTask.float32 = data;
    plcTaskList.try_enqueue(subTask);
}

void PlcDataProcess::setPlcData()
{
    if(!PLCCONNECTER.getConnectStatus())
    {
        while (plcTaskList.peek())
            plcTaskList.pop();
        return;
    }

    if(!plcTaskList.peek())
        return;

    PLCTask task;
    plcTaskList.try_dequeue(task);
    unsigned int i=task.addressIndex>>4;
    switch (task.dataType) {
    case BIT : {
         PLCCONNECTER.writeRegisters(i,1,setDByteFromBit(task.addressIndex,task.bit));
    } break;
    case INT_16 : {
        PLCCONNECTER.writeRegisters(i,1,(uint16_t*)&task.int16);
    } break;
    case FLOAT_32 : {
        uint16_t src[2] ;
        PLCCONNECTER.setFloatFromDDByte(task.float32,src);
        PLCCONNECTER.writeRegisters(i,2,src);
    } break;
    default:break;
    }
}

/***private function***/
uint16_t PlcDataProcess::getBitFromDByte(uint16_t* src,uint8_t loc)
{
    unsigned int mask=1<<loc;
    return (*src&mask)>>loc;
}

uint16_t PlcDataProcess::getBitsFromDByte(uint16_t* src,uint8_t loc,uint8_t nb)
{
    unsigned int mask=1<<loc;
    int length=nb;
    unsigned tmp=mask;
    int j=1;
    while (j<length){
        tmp=tmp<<1;
        tmp=tmp+mask;
        j++;
    }
    return (*src&tmp)>>loc;
}

int32_t PlcDataProcess::getLongfromDDByte(uint16_t* src)
{
    return src[1]+(src[0]<<16);
}

uint16_t* PlcDataProcess::setDByteFromBit(AddressIndex ind , bool data)
{
    unsigned int i=ind>>4;
    unsigned int bit_loc=ind&0x0f;

    PLCCONNECTER.readRegisters(i,1,&PlcSingleWriteData);

    if(data)
        PlcSingleWriteData = PlcSingleWriteData|(1<<bit_loc);
    else
        PlcSingleWriteData = PlcSingleWriteData&~(1<<bit_loc);
    return &PlcSingleWriteData;
}

void PlcDataProcess::setDDByteFromLong(long src, uint16_t* des)
{
    des[1]=src&0xFFFF;
    des[0]=src>>16;
}

void PlcDataProcess::swapDDByte(uint16_t* src, uint16_t* des)
{
    des[1]=src[0];
    des[0]=src[1];
}

}
